/*
 * @Description: 里程计信息发布
 * @Author: Zhijian Qiao
 * @Date: 2020-02-06 21:11:44
 */
#include "publisher/odometry_publisher.hpp"
#include "CommonFunc.h"

namespace avp_slam {
    OdometryPublisher::OdometryPublisher(ros::NodeHandle &nh,
                                         std::string topic_name,
                                         std::string base_frame_id,
                                         std::string child_frame_id,
                                         int buff_size)
            : nh_(nh) {

        publisher_ = nh_.advertise<nav_msgs::Odometry>(topic_name, buff_size);
        odometry_.header.frame_id = base_frame_id;
        odometry_.child_frame_id = child_frame_id;
    }

    void OdometryPublisher::Publish(const Eigen::Matrix4f &transform_matrix, double time) {
        ros::Time ros_time(time);
        PublishData(transform_matrix, ros_time);
    }

    void OdometryPublisher::Publish(const Eigen::Matrix4f &transform_matrix, double time, double start, double end) {
        ros::Time ros_time(time);

        odometry_.header.stamp = ros_time;
        //set the position
        odometry_.pose.pose.position.x = transform_matrix(0, 3);
        odometry_.pose.pose.position.y = transform_matrix(1, 3);
        odometry_.pose.pose.position.z = transform_matrix(2, 3);

        Eigen::Quaternionf q;
        q = transform_matrix.block<3, 3>(0, 0);
        q = q.normalized();
        odometry_.pose.pose.orientation.x = q.x();
        odometry_.pose.pose.orientation.y = q.y();
        odometry_.pose.pose.orientation.z = q.z();
        odometry_.pose.pose.orientation.w = q.w();

        odometry_.twist.twist.angular.x = start;
        odometry_.twist.twist.angular.y = end;

        publisher_.publish(odometry_);

    }

    void OdometryPublisher::Publish(const Eigen::Matrix4f &transform_matrix) {
        PublishData(transform_matrix, ros::Time::now());
    }

    void OdometryPublisher::PublishData(const Eigen::Matrix4f &transform_matrix, ros::Time time) {
        odometry_.header.stamp = time;

        //set the position
        odometry_.pose.pose.position.x = transform_matrix(0, 3);
        odometry_.pose.pose.position.y = transform_matrix(1, 3);
        odometry_.pose.pose.position.z = transform_matrix(2, 3);

        Eigen::Quaternionf q;
        q = transform_matrix.block<3, 3>(0, 0);
        q = q.normalized();
        odometry_.pose.pose.orientation.x = q.x();
        odometry_.pose.pose.orientation.y = q.y();
        odometry_.pose.pose.orientation.z = q.z();
        odometry_.pose.pose.orientation.w = q.w();

        double roll, pitch, yaw;
        toEulerAngle(q.cast<double>(), roll, pitch, yaw);

        if (first) {
            odometry_.twist.twist.linear.x = 0.0;
            odometry_.twist.twist.linear.y = 0.0;
            odometry_.twist.twist.linear.z = 0.0;
            odometry_.twist.twist.angular.x = 0.0;
            odometry_.twist.twist.angular.y = 0.0;
            odometry_.twist.twist.angular.z = 0.0;
            first = false;
        } else {
            double diffTime = (time - time_last_).toSec();
            odometry_.twist.twist.linear.x =
                    (odometry_.pose.pose.position.x - odometry_last_.pose.pose.position.x) / diffTime;
            odometry_.twist.twist.linear.y =
                    (odometry_.pose.pose.position.y - odometry_last_.pose.pose.position.y) / diffTime;
            odometry_.twist.twist.linear.z =
                    (odometry_.pose.pose.position.z - odometry_last_.pose.pose.position.z) / diffTime;
            odometry_.twist.twist.angular.x = (roll - roll_last) / diffTime;
            odometry_.twist.twist.angular.y = (pitch - pitch_last) / diffTime;
            odometry_.twist.twist.angular.z = (yaw - yaw_last) / diffTime;
        }


        roll_last = roll;
        pitch_last = pitch;
        yaw_last = yaw;
        odometry_last_ = odometry_;
        time_last_ = time;

        publisher_.publish(odometry_);
    }

    bool OdometryPublisher::HasSubscribers() {
        return publisher_.getNumSubscribers() != 0;
    }
}